#include "WiiChuck.h"

/*
 * Nunchuck functions  -- Talk to a Wii Nunchuck
 *
 * 2010 Szymon Kobalczyk, http://geekswithblogs.net/kobush/
 *  
 * Adapted library from the Bionic Arduino course : 
 *  http://todbot.com/blog/bionicarduino/
 *
 * 2007 Tod E. Kurt, http://todbot.com/blog/
 *
 * The Wii Nunchuck reading code originally from Windmeadow Labs
 *   http://www.windmeadow.com/node/42
 */
 
// soure: 
// http://www.windmeadow.com/node/42
// http://forums.nxtasy.org/index.php?showtopic=3075
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers
// http://web.archive.org/web/20080627223429/http://www.jmaginary.com/wiiNunchuck.cpp.txt
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html

/*
Y: 493,00 | Accel: Z 493,00 * For scaling the raw values from the nunchuck into G values
 * Details on callibration and the maths can be found at
 *    http://www.wiili.org/index.php/Motion_analysis
 
     Zero Points
       <math>x_0 = (x_1 + x_2) / 2\,</math>
       <math>y_0 = (y_1 + y_3) / 2\,</math>
       <math>z_0 = (z_2 + z_3) / 2\,</math>
     
     One G points
       <math>x = \frac{x_{raw} - x_0}{x_3 - x_0}</math>
       <math>y = \frac{y_{raw} - y_0}{y_2 - y_0}</math>
       <math>z = \frac{z_{raw} - z_0}{z_1 - z_0}</math>
       
 * Not all of these are used and could be deleted (kept to make interpretting math&apos;s easier 
 * 0-Zero G Value 
 * 1-Value when laying on table 
 * 2-Value when resting on nose
 * 3-Value when resting on side (left side up)
*/

// Horizontal with the joystick facing up
#define X1 512 //500
#define Y1 512 //481
#define Z1 749 //785

// Resting on nose with power cord facing up
#define X2 512 //500
#define Y2 727 //621
#define Z2 512 //575

// Laying on its side, so the left side is facing up
#define X3 716 //711
#define Y3 512 //449
#define Z3 512 //582

// zero G points
#define X0 ((X1+X2) / 2)
#define Y0 ((Y1+Y3) / 2)
#define Z0 ((Z2+Z3) / 2)


// Wii Nunchuck constants
#define xAnalogLo 74
#define xAnalogNu 128
#define xAnalogHi 222

#define yAnalogLo 28
#define yAnalogNu 133
#define yAnalogHi 230

#define xAccelLo 490
#define xAccelNu 852
#define xAccelHi 890

#define yAccelLo 280
#define yAccelNu 488
#define yAccelHi 700

#define zAccelLo 296
#define zAccelNu 504
#define zAccelHi 708


// Constructor
WiiNunchuck::WiiNunchuck(int statusLED, int cLED, int zLED) 
    : _statusLED(statusLED), _cLED(cLED), _zLED(zLED)
{
  if( _statusLED != 0 ) pinMode(_statusLED, OUTPUT);
  if( _cLED != 0 ) pinMode(_cLED, OUTPUT);
  if( _zLED != 0 ) pinMode(_zLED, OUTPUT);
}

// Uses port C (analog in) pins as power & ground for Nunchuck
void WiiNunchuck::setPowerPins()
{
#define pwrpin PORTC3
#define gndpin PORTC2
  DDRC |= _BV(pwrpin) | _BV(gndpin);
  PORTC &=~ _BV(gndpin);
  PORTC |=  _BV(pwrpin);
  delay(100);  // wait for things to stabilize        
}

// initialize the I2C system, join the I2C bus,
// and tell the nunchuck we're talking to it
void WiiNunchuck::init()
{
  Wire.begin();                 // join i2c bus as master
  Wire.beginTransmission(0x52); // transmit to device 0x52
  Wire.send(0x40);              // sends memory address
  Wire.send(0x00);              // sends sent a zero.  
  Wire.endTransmission();       // stop transmitting
}

// Send a request for data to the nunchuck
// (send zero to reset data transfer)
void WiiNunchuck::sendRequest()
{
  Wire.beginTransmission(0x52); // transmit to device 0x52
  Wire.send(0x00);              // sends one byte
  Wire.endTransmission();       // stop transmitting
}


// Encode data to format that most wiimote drivers except
// only needed if you use one of the regular wiimote drivers
unsigned int WiiNunchuck::decodeByte(unsigned int x)
{
  x = (x ^ 0x17) + 0x17;
  return x;
}

// Receive data back from the nunchuck, 
// returns 1 on successful read. returns 0 on failure
int WiiNunchuck::getData()
{
  int count = 0;

  Wire.requestFrom (0x52, 6);		// request data from nunchuck
  while( Wire.available() )
  {
    // receive byte as an integer
    nunchuck_buf[count] = decodeByte( Wire.receive() );		
    digitalWrite(_statusLED, HIGH);	// signal led
    count++;
  }
  digitalWrite(_statusLED, LOW); 	
    
  sendRequest();  // send request for next data payload

  // If we recieved the 6 bytes, then go print them
  if( count >= 5 )
  {
    extractData();
    return 1;   // success
  }

  return 0; //failure
}

// Extract data from data buffer
void WiiNunchuck::extractData()
{
  // Read z button
  bool zButton = ((nunchuck_buf[5] & 0x01) != 0);
  zButtonHit = false;
  if( zButton && !zButtonDown ) zButtonHit = true;
  zButtonDown = zButton;
    
  // signal Z button status
  if( zButton && _zLED != 0 ) digitalWrite(_zLED, HIGH); 
  else digitalWrite(_zLED, LOW);

  // Read c button
  bool cButton = ((nunchuck_buf[5] & 0x02) != 0);
  cButtonHit = false;
  if( cButton && !cButtonDown ) cButtonHit = true;
  cButtonDown = cButton;
    
  // signal C button status
  if( cButton && _cLED != 0 ) digitalWrite(_cLED, HIGH); 
  else digitalWrite(_cLED, LOW);

  // Read analog data
  xAnalog = nunchuck_buf[0];
  yAnalog = nunchuck_buf[1];
  xAccel = nunchuck_buf[2] << 2; 
  yAccel = nunchuck_buf[3] << 2;
  zAccel = nunchuck_buf[4] << 2;

  // byte nunchuck_buf[5] contains bits for z and c buttons
  // it also contains the least significant bits for the accelerometer data
  // so we have to check each bit of byte outbuf[5]

  // Add significant bits
  xAccel |= (nunchuck_buf[5] >> 2) & 0x03;
  yAccel |= (nunchuck_buf[5] >> 4) & 0x03;
  zAccel |= (nunchuck_buf[5] >> 6) & 0x03;
}

// Get x analog as float
float WiiNunchuck::getAnalogX()
{
  if( xAnalog <= xAnalogNu )
    return ((float)xAnalog-xAnalogNu)/(xAnalogNu-xAnalogLo);
  else
    return ((float)xAnalog-xAnalogNu)/(xAnalogHi-xAnalogNu);
}

// Get y analog as float
float WiiNunchuck::getAnalogY()
{
  if( yAnalog <= yAnalogNu )
    return ((float)yAnalog-yAnalogNu)/(yAnalogNu-yAnalogLo);
  else
    return ((float)yAnalog-yAnalogNu)/(yAnalogHi-yAnalogNu);
}

// Get x acceleration as float
float WiiNunchuck::getAccelerationX()
{
  if ( xAccel <= xAccelNu )
    return ((float)xAccel - xAccelNu) / (xAccelNu - xAccelLo);
  else
    return ((float)xAccel - xAccelNu) / (xAccelHi - xAccelNu);
}

// Get y acceleration as float
float WiiNunchuck::getAccelerationY()
{
  if( yAccel <= yAccelNu )
    return ((float)yAccel-yAccelNu)/(yAccelNu-yAccelLo);
  else
    return ((float)yAccel-yAccelNu)/(yAccelHi-yAccelNu);
}

// Get z acceleration as float
float WiiNunchuck::getAccelerationZ()
{
  if( zAccel <= zAccelNu )
    return ((float)zAccel-zAccelNu)/(zAccelNu-zAccelLo);
  else
    return ((float)zAccel-zAccelNu)/(zAccelHi-zAccelNu);
}

//Calculates and returns the xAxis acceleration in Gs
float WiiNunchuck::getXGs(){
  return ((float)xAccel - X0) / (X3 - X0);
}

//Calculates and returns the yAxis acceleration in Gs
float WiiNunchuck::getYGs(){
  return ((float)yAccel - Y0) / (Y2 - Y0);
}

//Calculates and returns the zAxis acceleration in Gs
float WiiNunchuck::getZGs(){
  return ((float)zAccel - Z0) / (Z1 - Z0);  
}

void WiiNunchuck::printData() 
{
    //Serial.print(i,DEC);
    //Serial.print("\t");

    Serial.print("joy:");
    Serial.print(xAnalog,DEC);
    Serial.print(",");
    Serial.print(yAnalog, DEC);
    Serial.print(" -> ");
    Serial.print(getAnalogX()*100.0f,DEC);
    Serial.print(",");
    Serial.print(getAnalogY()*100.0f, DEC);
    Serial.print("  \t");

    Serial.print("acc:");
    Serial.print(xAccel, DEC);
    Serial.print(",");
    Serial.print(yAccel, DEC);
    Serial.print(",");
    Serial.print(zAccel, DEC);
    Serial.print("\t");

    Serial.print("Gs:");
    Serial.print(getXGs() * 100.0f, DEC);
    Serial.print(",");
    Serial.print(getYGs() * 100.0f, DEC);
    Serial.print(",");
    Serial.print(getZGs() * 100.0f, DEC);
    Serial.print("\t");

    Serial.print("but:");
    Serial.print(zButtonDown, DEC);
    Serial.print(",");
    Serial.print(cButtonDown, DEC);

    Serial.println();
}